﻿using System.Collections.Generic;

namespace Raven
{
    public class GraphSearchDijkstra<N, E> where N : GraphNode where E : GraphEdge
    {
        private SparseGraph<N, E> m_Graph;
        private E[] m_ShortestPathTree;
        private Fix[] m_CostToThisNode;
        private E[] m_SearchFrontier;

        private int m_iSource;
        private int m_iTarget;

        public GraphSearchDijkstra(SparseGraph<N, E> graph, int source, int target = -1)
        {
            m_Graph = graph;
            m_ShortestPathTree = new E[graph.NumNodes()];
            m_SearchFrontier = new E[graph.NumNodes()];
            m_CostToThisNode = new Fix[graph.NumNodes()];
            m_iSource = source;
            m_iTarget = target;

            Search();
        }


        private void Search()
        {
            //create an indexed priority queue that sorts smallest to largest
            //(front to back).Note that the maximum number of elements the iPQ
            //may contain is N. This is because no node can be represented on the 
            //queue more than once.
            IndexedPriority<Fix> pq =
                new IndexedPriority<Fix>(m_CostToThisNode, Comparer<Fix>.Default, m_Graph.NumNodes());

            //put the source node on the queue
            pq.Insert(m_iSource);

            //while the queue is not empty
            while (!pq.Empty())
            {
                //get lowest cost node from the queue. Don't forget, the return value
                //is a *node index*, not the node itself. This node is the node not already
                //on the SPT that is the closest to the source node
                int NextClosestNode = pq.Pop();

                //move this edge from the frontier to the shortest path tree
                m_ShortestPathTree[NextClosestNode] = m_SearchFrontier[NextClosestNode];

                //if the target has been found exit
                if (NextClosestNode == m_iTarget) return;

                //now to relax the edges.
                LinkedList<E> edges = m_Graph.GetEdges(NextClosestNode);

                //for each edge connected to the next closest node
                foreach (var pE in edges)
                {
                    //the total cost to the node this edge points to is the cost to the
                    //current node plus the cost of the edge connecting them.
                    Fix NewCost = m_CostToThisNode[NextClosestNode] + pE.Cost();

                    //if this edge has never been on the frontier make a note of the cost
                    //to get to the node it points to, then add the edge to the frontier
                    //and the destination node to the PQ.
                    if (m_SearchFrontier[pE.To()] == null)
                    {
                        m_CostToThisNode[pE.To()] = NewCost;

                        pq.Insert(pE.To());

                        m_SearchFrontier[pE.To()] = pE;
                    }

                    //else test to see if the cost to reach the destination node via the
                    //current node is cheaper than the cheapest cost found so far. If
                    //this path is cheaper, we assign the new cost to the destination
                    //node, update its entry in the PQ to reflect the change and add the
                    //edge to the frontier
                    else if ((NewCost < m_CostToThisNode[pE.To()]) &&
                             (m_ShortestPathTree[pE.To()] == null))
                    {
                        m_CostToThisNode[pE.To()] = NewCost;

                        //because the cost is less than it was previously, the PQ must be
                        //re-sorted to account for this.
                        pq.ChangePriority(pE.To());

                        m_SearchFrontier[pE.To()] = pE;
                    }
                }
            }
        }

        public List<E> GetSPT()
        {
            return new List<E>(m_ShortestPathTree);
        }

        public LinkedList<int> GetPathToTarget()
        {
            LinkedList<int> path = new LinkedList<int>();

            //just return an empty path if no target or no path found
            if (m_iTarget < 0) return path;

            int nd = m_iTarget;

            path.AddFirst(nd);

            while ((nd != m_iSource) && (m_ShortestPathTree[nd] != null))
            {
                nd = m_ShortestPathTree[nd].From();

                path.AddFirst(nd);
            }

            return path;
        }

        public Fix GetCostToTarget()
        {
            return m_CostToThisNode[m_iTarget];
        }

        public Fix GetCostToNode(int nd)
        {
            return m_CostToThisNode[nd];
        }
    }
}